#include <ros/ros.h>
#include <ros/network.h>
#include <string>
#include <std_msgs/String.h>
#include <sstream>
#include "../include/ros_robot/qnode.hpp"
namespace ros_robot {

    QNode::QNode(int argc, char** argv ) :init_argc(argc),init_argv(argv){}

    QNode::~QNode() {

        if(ros::isStarted()) {
            ros::shutdown();
            ros::waitForShutdown();
        }
        wait();
    }

    bool QNode::init() {

        ros::init(init_argc,init_argv,"ros_robot");
        if ( ! ros::master::check() ) {
            return false;
        }
        ros::start();
        ros::NodeHandle n;

        cmd_vel_publisher=n.advertise<geometry_msgs::Twist>("cmd_vel",1000);//turtlebot3
        //cmd_vel_publisher = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1000);//turtlesim
        
        odom_sub=n.subscribe("odom",1000,&QNode::odom_callBack,this);//turtlebot3
        //pose_sub = n.subscribe("/turtle1/pose",1000,&QNode::pose_callBack,this);//turtlesim
        
        power_subscriber=n.subscribe("power",1000,&QNode::power_callBack,this);

        start();
        return true;
    }

    bool QNode::init(const std::string &master_url, const std::string &host_url) {

        std::map<std::string,std::string> remappings;
        remappings["__master"] = master_url;
        remappings["__hostname"] = host_url;
        ros::init(remappings,"ros_robot");
        if ( ! ros::master::check() ) {
            return false;
        }
        ros::start();
        ros::NodeHandle n;

        cmd_vel_publisher=n.advertise<geometry_msgs::Twist>("cmd_vel",1000);//turtlebot3
        //cmd_vel_publisher = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1000);//turtlesim
        
        odom_sub=n.subscribe("odom",1000,&QNode::odom_callBack,this);//turtlebot3
        //pose_sub = n.subscribe("/turtle1/pose",1000,&QNode::pose_callBack,this);//turtlesim
        power_subscriber=n.subscribe("power",1000,&QNode::power_callBack,this);

        start();
        return true;
     }

    void QNode::run() {

        //ros::Rate loop_rate(1);
        while( ros::ok() ) {
          
        }
        Q_EMIT rosShutdown(); //用于指示GUI关闭(对roslaunch很有用)
    }

    void QNode::set_cmd_vel(char key,double linear,double angular){
        std::map<char, std::vector<float> > moveBindings{
            {'u', {1, 0, 0, 1}},
            {'i', {1, 0, 0, 0}},
            {'o', {1, 0, 0, -1}},

            {'j', {0, 0, 0, 1}},
            {'l', {0, 0, 0, -1}},

            {'m', {-1, 0, 0, -1}},
            {',', {-1, 0, 0, 0}},
            {'.', {-1, 0, 0, 1}},

            {'U', {1, 1, 0, 0}},
            {'I', {1, 0, 0, 0}},
            {'O', {1, -1, 0, 0}},

            {'J', {0, 1, 0, 0}},
            {'L', {0, -1, 0, 0}},

            {'M', {-1, 1, 0, 0}},
            {'<', {-1, 0, 0, 0}},
            {'>', {-1, -1, 0, 0}},

            {'t', {0, 0, 1, 0}},
            {'b', {0, 0, -1, 0}},

            {'k', {0, 0, 0, 0}},
            {'K', {0, 0, 0, 0}}
        };

        double x = moveBindings[key][0];
        double y = moveBindings[key][1];
        double z = moveBindings[key][2];
        double th = moveBindings[key][3];

        geometry_msgs::Twist twist;
        twist.linear.x = x * linear;
        twist.linear.y = y * linear;
        twist.linear.z = z * linear;

        twist.angular.x = 0;
        twist.angular.y = 0;
        twist.angular.z = th * angular;

        cmd_vel_publisher.publish(twist);
    }

    /*void QNode::pose_callBack(const turtlesim::Pose& msg){//turtlesim
        Q_EMIT speed_vel(msg.linear_velocity,msg.angular_velocity);
    }*/
    
    void QNode::odom_callBack(const nav_msgs::Odometry &msg){//turtlebot3
        Q_EMIT speed_vel(msg.twist.twist.linear.x,msg.twist.twist.linear.y);
    }

    void QNode::power_callBack(const std_msgs::Float32& msg){
        //Q_EMIT power_vel(msg.data);

        float p = 90.012;
        Q_EMIT power_vel(p);
    }

    void QNode::sub_image(QString topic_name){//add
        ros::NodeHandle n;
        image_transport::ImageTransport it_(n);
        image_subscriber = it_.subscribe(topic_name.toStdString(),1000,&QNode::image_callBack,this);
    }
    
    void QNode::image_callBack(const sensor_msgs::ImageConstPtr& msg){//add
        cv_bridge::CvImageConstPtr cv_Ptr;
        cv_Ptr=cv_bridge::toCvCopy(msg,msg->encoding);
        QImage im = Mat2QImage((cv_Ptr->image));
        Q_EMIT image_val(im);
    }

    QImage QNode::Mat2QImage(cv::Mat const& src){
        QImage dest(src.cols, src.rows, QImage::Format_ARGB32);
        const float scale = 255.0;

       if (src.depth() == CV_8U) {
         if (src.channels() == 1) {
           for (int i = 0; i < src.rows; ++i) {
             for (int j = 0; j < src.cols; ++j) {
               int level = src.at<quint8>(i, j);
               dest.setPixel(j, i, qRgb(level, level, level));
             }
           }
         } else if (src.channels() == 3) {
           for (int i = 0; i < src.rows; ++i) {
             for (int j = 0; j < src.cols; ++j) {
               cv::Vec3b bgr = src.at<cv::Vec3b>(i, j);
               dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
             }
           }
         }
       } else if (src.depth() == CV_32F) {
         if (src.channels() == 1) {
           for (int i = 0; i < src.rows; ++i) {
             for (int j = 0; j < src.cols; ++j) {
               int level = scale * src.at<float>(i, j);
               dest.setPixel(j, i, qRgb(level, level, level));
             }
           }
         } else if (src.channels() == 3) {
           for (int i = 0; i < src.rows; ++i) {
             for (int j = 0; j < src.cols; ++j) {
               cv::Vec3f bgr = scale * src.at<cv::Vec3f>(i, j);
               dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
             }
           }
         }
       }
       return dest;
    }

}
